#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>


using namespace cv;
using namespace std;


class ExponentialMovingAverageFilter
{
private:
    double alpha;                // 滑动系数，0 <= alpha <= 1
    double filteredValue = -1.0; // 上一次过滤后的值

public:
    // 构造函数，传入滑动系数alpha
    ExponentialMovingAverageFilter(double alpha) : alpha(alpha) {}

    // 对新的数据点进行滤波处理
    double filter(double newValue)
    {
        filteredValue = alpha * newValue + (1 - alpha) * filteredValue;
        return filteredValue;
    }

    // 初始化第一个数据点（没有历史滤波值时）
    void initializeWithFirstValue(double firstValue)
    {
        filteredValue = firstValue;
    }

    double get_firstValue()
    {
        return filteredValue;
    }
};

std::vector<cv::Point2d> processImagePart(int start_h, int end_h, cv::Mat &gray, cv::Mat &Gauss_img, int threshold)
{

    Mat hessian(2, 2, CV_32FC1);
    Mat eValue;
    Mat eVectors;
    std::vector<cv::Point2d> all_lines_points;

    double dxx_val = 0.0, dxy_val = 0.0, dyy_val = 0.0, dx_val = 0.0, dy_val = 0.0;
    for (int i = start_h; i < end_h; i+=1)
    {
        for (int j = 0; j < gray.cols; j++)
        {
            if (i > 0 && j > 0 && i < gray.rows - 1 && j < gray.cols - 1)
            {
                if (gray.at<uchar>(i, j) > threshold)
                {
                    // 1-1
                    dx_val = Gauss_img.at<float>(i, j) - Gauss_img.at<float>(i, j + 1);
                    // 1
                    //-1
                    dy_val = Gauss_img.at<float>(i, j) - Gauss_img.at<float>(i + 1, j);
                    // 1-2+1
                    dxx_val = Gauss_img.at<float>(i, j - 1) - 2 * Gauss_img.at<float>(i, j) + Gauss_img.at<float>(i, j + 1);
                    // 1
                    //-2
                    // 1
                    dyy_val = Gauss_img.at<float>(i - 1, j) - 2 * Gauss_img.at<float>(i, j) + Gauss_img.at<float>(i + 1, j);
                    // 1-1
                    //-1+1
                    dxy_val = Gauss_img.at<float>(i, j) - Gauss_img.at<float>(i, j + 1) - Gauss_img.at<float>(i + 1, j) + Gauss_img.at<float>(i + 1, j + 1);

                    hessian.at<float>(0, 0) = dxx_val;
                    hessian.at<float>(0, 1) = dxy_val;
                    hessian.at<float>(1, 0) = dxy_val;
                    hessian.at<float>(1, 1) = dyy_val;

                    eigen(hessian, eValue, eVectors);
                    double nx, ny;
                    double fmaxD = 0;
                    if (fabs(eValue.at<float>(0, 0)) >= fabs(eValue.at<float>(1, 0))) // 求特征值最大时对应的特征向量
                    {
                        nx = eVectors.at<float>(0, 0);
                        ny = eVectors.at<float>(0, 1);
                        fmaxD = eValue.at<float>(0, 0);
                    }
                    else
                    {
                        nx = eVectors.at<float>(1, 0);
                        ny = eVectors.at<float>(1, 1);
                        fmaxD = eValue.at<float>(1, 0);
                    }
                    double t = -(nx * dx_val + ny * dy_val) / (nx * nx * dxx_val + 2 * nx * ny * dxy_val + ny * ny * dyy_val);
                    if (fabs(t * nx) <= 0.5 && fabs(t * ny) <= 0.5)
                    {
                        all_lines_points.emplace_back(cv::Point2d(j, i));
                    }
                }
            }
        }
    }

    return all_lines_points;
}



vector<cv::Point2d> Find_Line(Mat image)
{
int width = 12;
int threshold = 81;
    std::vector<cv::Point2d> all_lines_points;
    Mat gray = image.clone();
    Mat img,gauss_img;
    // 高斯滤波
    gray.convertTo(img, CV_32FC1); // 转换数据类型不同的mat，改变位深度，32位浮点型单通道
    GaussianBlur(img, gauss_img, Size(0, 0), width/ 1.732, width / 1.732);
    // hessian矩阵
    int img_w = img.cols;
    int img_h = img.rows;
    auto start = std::chrono::high_resolution_clock::now();
    all_lines_points = processImagePart(0, img_h, gray, gauss_img, threshold);
    auto end = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
    // std::cout << "高斯运行时间为: " << duration.count()/1000 << "毫秒" << std::endl;
    // sort(all_lines_points.begin(), all_lines_points.end(), comp_p);

    // 在原图上显示提取结果
    // show_draw(img1, Pt);

    return all_lines_points;
}


int main(int argc, char** argv)
{
    string image_path = "C:\\Users\\dell\\Desktop\\105\\left_right\\r_1.png";

    // 使用cv::imread函数读取图片，第二个参数为图像读取模式
    // cv::IMREAD_COLOR: 加载彩色图像，任何透明度都将被忽视。它是默认标志。
    // cv::IMREAD_GRAYSCALE: 以灰度模式加载图像
    // cv::IMREAD_UNCHANGED: 加载图像，包括alpha通道
    cv::Mat image = cv::imread(image_path, cv::IMREAD_COLOR);
    Mat gray;
    cvtColor(image, gray, COLOR_BGR2GRAY);
    // 检查是否成功读取图片
    if (image.empty())
    {
        std::cout << "Could not read the image: " << argv[1] << std::endl;
        return -1;
    }
    ExponentialMovingAverageFilter emaFilter_left(1.0);  // 假设选择滑动系数为0.8
    ExponentialMovingAverageFilter emaFilter_right(1.0); // 假设选择滑动系数为0.8
    std::vector<cv::Point2d> left_key_points = Find_Line(gray);
    std::vector<cv::Point2d> right_key_points = Find_Line(gray);
 // 对点坐标进行指数移动滤波
        emaFilter_left.initializeWithFirstValue(left_key_points[0].x);
        // 坐标按照Y轴 从小到大排列并去除间隔较小的点

        for (int i = 0; i < left_key_points.size() - 1; i++)
        {
            if (left_key_points[i + 1].y - left_key_points[i].y > 0.8)
            {
                left_key_points.emplace_back(emaFilter_left.filter(left_key_points[i].x), left_key_points[i].y);
            }
        }
        emaFilter_right.initializeWithFirstValue(right_key_points[0].x);
        for (int i = 0; i < right_key_points.size() - 1; i++)
        {
            if (right_key_points[i + 1].y - right_key_points[i].y > 0.8)
            {
                right_key_points.emplace_back(emaFilter_right.filter(right_key_points[i].x), right_key_points[i].y);
            }
        }

        // 是否显示提取的激光线
        if (1)
        {
            // 创建一个Mat对象作为画布
            cv::Mat image_l(1281, 1025, CV_8UC1, Scalar(0)); // 创建一个500x500白色背景图像
            cv::Mat image_r(1281, 1025, CV_8UC1, Scalar(0)); // 创建一个500x500白色背景图像
            for (int i = 0; i < left_key_points.size(); i++)
            {
                image_l.at<uchar>(int(left_key_points[i].y), int(left_key_points[i].x)) = 255;
            }
            for (int i = 0; i < right_key_points.size(); i++)
            {
                image_r.at<uchar>(int(right_key_points[i].y), int(right_key_points[i].x)) = 255;
            }
    cv::namedWindow("Left", cv::WINDOW_NORMAL); // 创建一个可以调整大小的窗口
    cv::namedWindow("Right", cv::WINDOW_NORMAL); // 创建一个可以调整大小的窗口

            imshow("Left", image_l);
            imshow("Right", image_r);
            waitKey(0);
        }


    // 使用cv::imshow函数显示图像，第一个参数是窗口名称，第二个参数是要显示的图像
    cv::namedWindow("Image", cv::WINDOW_NORMAL); // 创建一个可以调整大小的窗口
    cv::imshow("Image", image);

    // 等待用户按键，如果按下任意键则关闭窗口
    cv::waitKey(0); // 参数0表示无限等待

    // 最后，清理所有已创建的窗口
    cv::destroyAllWindows();

    return 0;
}